#include "point.hpp"
#include "map.hpp"

#include <cmath>
#include <iostream>
using namespace std;
using namespace dw;

// if \(mapstate.isBlock\(\{(.*?)\}\)\) return false;
// cout << Coord\((.*?)\) << endl;
// map\[\{(.*?)\}\] = Map::Color::GREEN;

// cout << Coord\((.*?)\) << endl;
// if (mapstate.isBlock({$1})) return false;
// map[{$1}] = mapstate.isBlock({$1}) ? Map::Color::RED : Map::Color::GREEN;

inline double slope(const Coord& p1, const Coord& p2) {
    if (p2.x == p1.x) {
        return 0;
    } else {
        return (double)(p2.y - p1.y) / (p2.x - p1.x);
    }
}

bool checkCrossWalkable(const Map::State &mapstate, const Coord &p1, const Coord &p2)
{
    const int row = mapstate.row;
    const int col = mapstate.col;

    double k = slope(p1, p2);
    double b = p1.y - k * p1.x;

    Map map(mapstate);

    if (k != 0)
    {
        if (k > 1 || k < -1)
        {
            b = -b / k;
            k = 1 / k;

            Coord lower = (p1.y < p2.y) ? p1 : p2;
            Coord upper = p1 + p2 - lower;

            for (int y = (int)floor(lower.y); y < (int)ceil(upper.y); ++y)
            {
                double x = k * y + b;
                int int_x = (int)floor(x);
                
                map[{int_x, y}] = mapstate.isBlock({int_x, y}) ? Map::Color::RED : Map::Color::GREEN;
                map[{int_x, y - 1}] = mapstate.isBlock({int_x, y - 1}) ? Map::Color::RED : Map::Color::GREEN;

                if (x < int_x + 0.25)
                {
                    map[{int_x - 1, y}] = mapstate.isBlock({int_x - 1, y}) ? Map::Color::RED : Map::Color::GREEN;
                    map[{int_x - 1, y - 1}] = mapstate.isBlock({int_x - 1, y - 1}) ? Map::Color::RED : Map::Color::GREEN;
                }
                else if (x > int_x + 0.75)
                {
                    map[{int_x + 1, y}] = mapstate.isBlock({int_x + 1, y}) ? Map::Color::RED : Map::Color::GREEN;
                    map[{int_x + 1, y - 1}] = mapstate.isBlock({int_x + 1, y - 1}) ? Map::Color::RED : Map::Color::GREEN;
                }
            }
        }
        else
        {
            Coord left = (p1.x < p2.x) ? p1 : p2;
            Coord right = p1 + p2 - left;

            for (int x = (int)floor(left.x); x < (int)ceil(right.x); ++x)
            {
                double y = k * x + b;
                int int_y = (int)floor(y);

                map[{x, int_y}] = mapstate.isBlock({x, int_y}) ? Map::Color::RED : Map::Color::GREEN;
                map[{x - 1, int_y}] = mapstate.isBlock({x - 1, int_y}) ? Map::Color::RED : Map::Color::GREEN;

                if (y < int_y + 0.25)
                {
                    map[{x, int_y - 1}] = mapstate.isBlock({x, int_y - 1}) ? Map::Color::RED : Map::Color::GREEN;
                    map[{x - 1, int_y - 1}] = mapstate.isBlock({x - 1, int_y - 1}) ? Map::Color::RED : Map::Color::GREEN;
                }
                else if (y > int_y + 0.75)
                {
                    map[{x, int_y + 1}] = mapstate.isBlock({x, int_y + 1}) ? Map::Color::RED : Map::Color::GREEN;
                    map[{x - 1, int_y + 1}] = mapstate.isBlock({x - 1, int_y + 1}) ? Map::Color::RED : Map::Color::GREEN;
                }
            }
        }
    }
    else
    {
        if (1E-2 > fabs(p1.x - p2.x))
        {
            Coord lower = (p1.y < p2.y) ? p1 : p2;
            Coord upper = p1 + p2 - lower;

            for (int y = (int)floor(lower.y); y < (int)ceil(upper.y); ++y)
            {
                int int_x = (int)floor(p1.x);
                map[{int_x, y}] = mapstate.isBlock({int_x, y}) ? Map::Color::RED : Map::Color::GREEN;

                if (p1.x > int_x + 0.75) {
                    map[{int_x + 1, y}] = mapstate.isBlock({int_x + 1, y}) ? Map::Color::RED : Map::Color::GREEN;
                }
                if (p1.x < int_x + 0.25) {
                    map[{int_x - 1, y}] = mapstate.isBlock({int_x - 1, y}) ? Map::Color::RED : Map::Color::GREEN;
                }
            }
        }
        else if (1E-2 > fabs(p1.y - p2.y))
        {
            Coord left = (p1.x < p2.x) ? p1 : p2;
            Coord right = p1 + p2 - left;

            for (int x = (int)floor(left.x); x < (int)ceil(right.x); ++x)
            {
                int int_y = (int)floor(p1.y);
                map[{x, int_y}] = mapstate.isBlock({x, int_y}) ? Map::Color::RED : Map::Color::GREEN;

                if (p1.y > int_y + 0.75) {
                    map[{x, int_y + 1}] = mapstate.isBlock({x, int_y + 1}) ? Map::Color::RED : Map::Color::GREEN;
                }
                if (p1.y < int_y + 0.25) {
                    map[{x, int_y - 1}] = mapstate.isBlock({x, int_y - 1}) ? Map::Color::RED : Map::Color::GREEN;
                }
            }
        }
    }

    map[p1] = Map::Color::PINK;
    map[p2] = Map::Color::BLUE;
    cout << map << endl;

    return true;
}

int main()
{

    Map map(64, 64);
    Map::State state = map.getMapState();

    vector<pair<Coord, Coord>> v = {
        {{0, 0}, {0, 63}},
        {{0, 0}, {1, 63}},
        {{0, 0}, {31, 63}},
        {{0, 0}, {63, 63}},
        {{0, 0}, {63, 31}},
        {{0, 0}, {63, 1}},
        {{0, 0}, {63, 0}},

        {{0, 63}, {63, 62}},
        {{0, 63}, {63, 31}},
        {{0, 63}, {63, 1}},
        {{0, 63}, {63, 0}},
        {{0, 63}, {1, 63}},
        {{0, 63}, {31, 63}},
        {{0, 63}, {62, 63}},

        {{0.5, 0.5}, {0.5, 63.5}},
        {{1.5, 1.5}, {62.5, 1.5}}
    };

    for (auto p : v) {
        Coord start = p.first;
        Coord end = p.second;

        cout << "from " << start << " to " << end << endl;
        checkCrossWalkable(state, start, end);
    }

    return 0;
}